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ACOUSTIC  DATA  PROCESSING  USING  THE 
DECENTRALIZED  SQUARE  ROOT  INFORMATION  FILTER 

i 

\ 

Project  Summary 


Very  Large  Scale  Integration  (VLSIj-  technology  has  been  developed  to  the 
point  where  special  purpose  processors  may  be  concatenated  to  form 
supercomputers  with  far  greater  throughput  rates  than  uniprocessor  machines. 
MTI  has  developed  a  parallel  form  of  the  conventional  Kalman  filter  that  is 
well  suited  to  being  implemented  in  a  multiprocessing  environment.  Moreover, 
our  Decentralized  Square  Root  Information  Filter  (DSRIF)  has  several  very 
unique  features  which  could  be  incorporated  into  the  design  of  an  integrated 
undersea  tracking  system  with  much  improved  performance  over  existing  methods. 


Phase  I  research  demonstrated  feasibility  of  the  DSRIF  as  a  means  for 
solving  the  linear  least  squares  estimation  problem  in  decentralized  form. 
Underwater  tracking  of  a  high  velocity  torpedo  (undergoing  high  dynamic 
maneuvers)  was  simulated.  Also,  an  extended  form  of  the  DSRIF  was  used  to 
complete  the  processing  of  real  Multiple  Rocket  Launch  System  Data  provided  by 
the  White  Sands  Missile  Range.  In  this  case,  an  adaptive  form  of  the  extended 
DSRIF,  wherein  the  process  noise  levels  wore  mode  to  be  o  function  of  the 


globally  optimal  estimate  error  covariance,  was  successfully  used  to  track  the 


rocket  data. 


V 
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1.0  Introduction  and  Problem  Statement 


The  trajectory  estimation  problem  is  a  problem  of  nonlinear  estimation. 
Assume  that  the  state  of  a  target  evolves  in  Lime  according  to  the  equation 

x(t)  =  f(x(t),u(t))  +  w(t)  (1) 

where  u(t)  is  the  target's  nominal  control  vector  and  w(t)  is  a  zero  mean 
white  noise  process  with  spectral  density  Q(t).  Values  for  the  latter  are 
selected  in  order  to  compensate  for  errors  in  the  model  which  may  originate 
from  unknown  perturbations  tc  the  nominal  control  (such  as  turbulence,  strong 
currents,  large  surface  waves,  etc.)  as  well  as  from  uncertainties  in  the 
hydrodynamics  of  the  irarget  vehicle.  The  corresponding  discrete  measurement 
vector  for  the  i^*^  sensor  is  given  by 

Yk  =  +  '^k  (2) 

where  v^  is  a  zero  mean  white  noise  sequence  with  covariance  R^.  The  problem 
is  to  estimate  the  target  states  x^  based  upon  all  of  the  past  measurements  yj 
where  {1<i<M  and  1<l<k}.  The  state  vector  x^  contains  the  target  position  and 
velocity,  biases  which  account  for  the  displacement  and  orientation  of  the 
sensor  or  "local"  coordinate  systems  (LCSs)  w.r.t.  the  global  coordinate 
system  (GCS),  and  acceleration  when  the  target  vehicle  is  maneuvering. 

Unfortunately,  the  optimal  nonlinear  estimator  (conditional  mean)  cannot 
be  realized  with  a  finite-dimensional  implementation  and  consequently,  all 
practical  nonlinear  filters  must  be  suboptimal.  The  usual  suboptimal  solution 
is  the  Conventional  Kalman  Filter  when  the  nominal  trajectory  is  known  o 
priori,  the  Extended  Kalman  Filter  when  the  nominal  trajectory  is  unavailable, 
and  Higher  Order  Filters  (such  as  th.  second  order  filter  [5],  the  single- 
stage  iterative  filter  [4],  and  the  Gaussian  sum  filter  [5],  among  others  [6]) 
when  even  greater  accuracy  is  desired.  The  tradeoff  here  is  performance 
versus  real-time  computational  requirement. 

Thus,  we  see  that  the  ability  of  a  sensor  group  to  accurotely  record  the 
motion  of  one  or  more  underwater  targets  is  a  function  of  the  individual 
target  and  sensor  dync-Tical  models  as  well  as  the  particular  olgorithm  used  to 
combine  raw  data  and  produce  track  estimates.  Another  issue  is  intersensor 
communication.  Sensors  which  operate  independently  from  one  another  will 
exhibit  larger  estimate  errors  than  ones  which  communicate  with  other  members 
of  the  network. 

Let  the  global  discrete  time  linear  system 

Xk  =  ^k-l  *  '^k-1  (5) 

where 

Wk  =  N(0,Qk)  (4) 

Xq  '  N('’.Po(-))  (5) 

be  the  target  dynamics  linearized  about  the  nominal  trajectory  (or  estimate), 
and  the  global  measurement  model 


where  v||  through  vjj  ore  uncorrelated  random  vectors  and 

vj  =  N{0,Ri)  (7) 

be  the  model  for  the  tracking  sensors.*  The  problem  is  to  calculate  the 

globally  optimal  (minimum  mean  square  error)  estimate  of  xi,  and  its  associated 

1  M  ^ 

estimate  error  covariance  matrix  when  through  yj^'  are  processed 
separately  by  locally  optimal  estimators  1  through  M  correspondingly.  The 
local  dynamical  models  are 

’^k  =  ^k-1  ^k-1  +  '^k-l 

where 

wj  =  N(O.Q^)  (9) 

xj  .  N(0.P§(-))  (10) 

and  the  local  measurement  models  are 

Yk  =  Hi  +  v^  (11) 

where  v^  satisfies  (7).  Notice  that  the  local  states  may  be  physically 
different  from  the  global  states.  Wilsky  et.al.  [7]  have  recently  shown  that 
a  necessary  and  sufficient  condition  for  our  being  able  to  recover  globally 
optimal  state  estimates  from  locally  optimal  ones  is  that 

ci  =  Hi  Mi  (12) 

and  for  the  tracking  application,  we  expect  that 

’‘k  '  '*'k  ’^k  (■’5) 

where  mJ  is  a  matrix  which  results  in  the  correct  partitioning  of  global 
states  to  the  subsystem  filters.  Alternatively,  equation  (12)  allows  us  to 
define  the  local  state  vector  in  terms  of  the  local  coordinate  system.  In 
this  case,  mJ  is  the  orthogonal  matrix  which  defines  the  transformation 
between  local  and  global  coordinate  systems. 

Lower  case  variables  are  vector  quantities  while  upper  case  generally 
corresponds  to  matrices  of  appropriate  dimension.  Also  w^,  xq,  and  vjj 
through  vJJ  are  uncorrelated  with  each  other  for  all  k  and  N(a,r) 
signifies  an  a  mean  white  Gaussian  process  with  covariance  matrix  P  . 

lA 


1.1  The  Decentralized  Square  Root  Information  Filter  as  a  Solution 


Decentralized  processing  is  achieved  by  distributing  the  minimization  of 
the  conventional  least  squares  performance  criterion  amongst  the  local  filters 
and  global  merging  equations  that  follow  (see  [1]  or  our  Phase  I  proposal  to 
DARPA  for  details).  Let 

(14) 

The  best  distribution  for  multisensor  tracking  is  probably  to  minimize 
II  xk  -  z^  11^  in  each  of  the  M  local  filters  and  minimize  the  remaining 
two  terms  in  the  performance  criterion  within  the  central  (merge)  processor. 
However,  this  point  will  be  explored  in  detail  in  Phase  II  research.  Thus, 
each  of  the  tracking  sensors  may  be  processed  using  the  standard  SRIF 
mechanization.  For  the  ith  sensor,  we  have 


Measurement  Update 


Wi 


Time  Update 


Rj(-) 
(Ri)-’-^  Hi 


2i(-) 


(Ri)"^  vi 


2r± 


Rv,(k) 

-Ri(  +  )  ik""' 


Ri(+)  Zk(+) 


0 

Ri(  +  )  zi(  +  ) 


(15) 


(i)  (i) 

Rw(k)  RwxO') 


(i) 


Rk+i(-) 


z:(k) 


(i) 


(i) 


(16) 


where 

[  Rj(-)  zj(-)  ]  -  [  0  0  ]  . 

[  Ry,(k)  ]  '  [  0  0  ]  , 

-1 

the  a  priori  estimate  {  Rq  (-)  Zq(-)  }  has  covariance 

-1  -T 

Po(->  -  Rq  (-)  Rq  (->  • 


(17) 

(18) 


(19) 
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(20) 


Qk  =  Rw  (k) 


(W)  . 


z^(k)  =  Ry,(k)  times  the  a  priori  expectation  of  Wk, 
correspond  to  normalized  measurement  equations  i.e., 

'k 


and  the  pair  {  Zk,  Hk 


Rk  = 


I.  Also, 


''(;i 


Tk  and 


are  orthogonal  transformations  which  put  the  matrices  on  the  left  hand 


sides  of  (15)  and  (16)  respectively  into  upper  triangular  form, 
implicitly  computed  using  Householder  transformations. 


They  may  be 


The  local  smoothing  coefficients  are  combined  with  process  noise  and 
prior  on  xq  by  solving  the  following  recursive  equation; 


(1) 


Rw(k) 

(M) 

R*(k) 

Rv„(k) 

-H*(k-1)  ik 


-1 


(1) 


Rwx(k) 


(M) 


Rwx(k) 


H-Ck-I)  Ik 


(1) 

z:(k) 

(M) 

z:(k) 

Zw(k) 

z*(k-1) 


Rw(k)  Rwx<k)  Zw(k) 


0 

0 


H^k)  z*(k) 

0  # 


(21) 


[  H^-l)  z*(-1)  ]  =  [  Ro(-)  zo(-)  ] 


(22) 


ond  H*(k)  is  upper  triangular.  To  obtain  the  globolly  optimal  information 
vector  Zk(+)  and  square  root  information  matrix  Rk(+),  we  solve  the  following 
equations  using  H*(k)  and  z*(k)  from  (21): 


(1)  (1) 
Rk(  +  ) 


(M)  (M) 

Rk(+)  Zk(+) 

H*(k-1)  z*(k-1) 


Rk(+)  Zk^+^ 

0  # 


(23) 
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where  is  an  orthogonal  transformation  which  puts  the  left  hand  side  of 
(23)  in  upper  triangular  form.  Globally  optimol  filter  estimates  and 
covariances  are  then  given  by 

-1 

x^(  +  )  =  (  +  )  2k(  +  )  (24) 

-1  -tr 

Pk(+)  =  Rk  (+)  Rk  (+)  (25) 

When  the  a  priori  information  about  the  initial  condition  and  process  noise 
models  are  adjusted,  it  is  only  necessary  to  rerun  (21)  and  (23)  without 
reprocessing  any  measurements. 

To  summarize,  the  measurements  from  each  tracking  sensor  may  be  processed 
by  a  local  SRIF 


(i) 

(15)-(20)  which  generates  a  set  of  smoothing  coefficients  R^  , 

(i)  (i)  (i) 

0  +  )  ,  Zv^(+)  OS  well  as  a  square  root  information  matrix  R(+)  and 

(i) 

information  vector  z(+)  ,  The  central  or  merge  processor  consists  of  three 

separate  processors  which  operate  in  parallel.  The  first  mechanizes  (21), (22) 
which  combines  the  local  smoothing  coefficients  with  the  effects  of  process 
noise  and  prior  information  about  the  initial  state.  The  second  mechanizes 
(23)  which  merges  the  local  square  root  information  matrices  and  vectors  with 
output  from  the  first,  but  only  upon  demand  by  the  third.  The  third  produces 
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estimates  and  covariances  whenever  desired  by  back-solving  (24)  and  (25) 
respectively,  noting  that  (24), (25)  require  output  from  the  second  processor. 
An  important  observation  is  that  feedbock  of  information  from  the  merge 
processor  to  the  local  filters  is  not  necessory  here.  This  helps  to  keep  the 
bandwidth  of  communication  between  the  local  processors  and  merge  processor 
from  exceeding  hardware  limitations. 


2.0  Results  of  the  Phase  I  Work 


In  order  to  validate  our  decentralized  approach  to  solving  the  linear 
least  squares  estimation  problem,  two  tasks  were  performed.  First,  in  the 
next  section  2.1  we  construct  and  execute  a  simulation  of  the  DSRIF  within  an 
underwater  test  range  environment.  The  nominal  trajectory  is  assumed  to  be 
known  a  priori  so  that  actually,  perturbotions  to  the  nominal  are  estimated. 
Then,  in  section  2.2  we  complete  our  processing  of  real  Multiple  Rocket  Launch 
System  Data  provided  by  the  White  Sands  Missile  Range.  In  this  case,  an 
adaptive  form  of  the  extended  DSRIF,  wherein  the  process  noise  levels  were 
chosen  to  bo  a  function  of  the  globally  optimal  estimate  error  covariance,  was 
used. 


2.1  Validation  of  the  DSRIF  for  a  Simulated 
Underwater  Zig-Zag  Maneuver 

A  low  level  simulation  of  an  underwater  multisensor  network,  tracking  a 
maneuvering  underwater  vehicle  was  encoded  in  Fortran  ’77  and  executed  on  on 
IBM  (clone)  Model  AT  desktop  computer  (640K  ram,  40  Mbyte  hard  disk,  Intel 
80287  math  coprocessor) ,  Initial  conditions  for  the  nominol  trajectory  were 
calculated  using  a  "flat  earth"  or  constant  gravitational  acceleration  model 
which  neglects  the  earth's  rotation,  neglects  hydrodynamic  drag,  and  assumes 
that  the  target  vehicle  is  a  point  mass. 

The  initial  position  is  located  within  the  polygon  formed  by  connecting 
adjacent  sensors,  and  the  desired  terminal  position  is  1.86  miles  downrange. 
However,  2  instantaneous  reversals  of  the  cross  range  velocity  at  42  and  124 
seconds  were  made,  and  this  resulted  in  a  total  path  length  of  2.62  miles 
when  projected  onto  the  "Range  Area  General  Map"  (RAGM)"  (which  we  use  to 
define  the  GCS).  That  is,  x\x^,x^  is  a  right  handed  coordinate  system 
where  the  vector  cross  product  of  with  x^  is  equol  to  x^,  and  the 

vectors  [x^  0  0]  and  [0  x^  0]  point  east  along  latitude  32.380  degrees 
and  north  along  longitude  106.481  degrees  respectively.  Then  [0  0  x^]  is 
collinear  with  the  radial  vector  which  points  outward  from  the  earth’s  center 
and  passes  through  the  origin  of  the  GCS.  Choosing  the  launch  elevation 
angle,  w.r.t.  the  x^.x^  tangent  plane,  to  bo  -3.96  degrees  results  in  a 
200  ft  depth  variation  over  the  path  length.  The  corresponding  initial 
position  and  velocity  is 
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r-  .  n 

—  ~ 

-o 

X 

7,920  feet 

CMO 

X 

5,280  feet 

xq 

0  feet 

xq 

= 

-4.14  feet/sec 

^0 

59.74  feet/sec 

v5 

59.74  feet/sec 

_  _ 

and  the  total  time  to  traverse  the  path  was  167  seconds. 


(26) 


Five  sensor  locations  which  surround  the  projected  flight  path  were 
selected.  Each  sensor  records  measurements  with  respect  to  its  own  LCS  so 
that  coordinate  transformations  to  the  6CS  were  derived  and  included  in  the 
observational  equations.  The  transformation  is 


r“ 

j 

r  • 

xl .  1 

=  (ji) 

xi,2 

xi.3 

_  _ 

I 

.  — 

(27) 


where 


fi  =  (  )  (  ®Ti  )  (  ) 


and 
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0  cos(L+aM 

0  -sin{L+a^) 


0 

sin(L+a^) 
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(28) 
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0 

0 

m 

0 

cos  L 

-sin 

L 

0 
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-sin  cos 
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0 

0 

1 


(32) 
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noting  that  =  (T^)“^  since  is  on  orthogonal  transformation.  A 

computational  savings  results  when  (^T^)(®T^){^T^)  are  combined  using 
trigonometric  identities  for  the  cosine  and  sine  of  the  sum  of  two  angles 
along  with  the  small  angle  approximation  since  is  bounded  by  +.025 

degrees  over  the  length  of  the  underwater  Test  Range. 


d^  Is  the  vector  from  the  GCS  to  the  ith  LCS  and  ore  the 

three  Euler  angles  describing  the  orientation  of  the  ith  LCS  w.r.t.  the  GCS. 
Specifically,  if  we  first  rotate  about  x^  counterclockwise  by  an  (L+a^) 
degree  change  in  latitude  (aligning  y^  with  the  polar  axis),  and  then  rotate 
counterclockwise  about  y^  by  a  degree  change  in  longitude,  (  is 

bounded  by  +.025  degrees  over  the  width  of  the  Test  Range)  then  rotate 
clockwise  about  by  an  L  degree  change  in  latitude,  and  finally  rotate 

about  by  degrees  to  account  for  "tangent  plane  misalignment",  the 

LCS  will  coincide  with  the  GCS  when  d^  =  0.  Is  equal  to  the  LCS  latitude 

-  GCS  latitude.  Is  equal  to  the  LCS  longitude  -  GCS  longitude. 


To  determine  a^,  given  L, 

solve  the  following  two  equations: 


and 


(in  global  coordinates). 


,xi-'' 

\i,2 

J  ■  T 


ECEF  ^ 


+  T9 

ECEF 


di-l 

di.2 

di'3 


(33) 


GCS 


where 
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°  2 

— 

•'e 
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cos  S 
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-•'e 

cos 
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sin  S 

ox^ 

''e 

sin 
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ECEF 

1— 

— 

(34) 


and 


t9 


sin  S  -cos  S  0 

i 

1 

1 

o 

o 

L 

cos  S  sin  S  0 

0  0  1 

0  sin  L  -cos  L 

0  cos  L  sin  L 

(35) 


r  •  ..“1 

„i,  1 

%i.2 

^r^  cos  {L+a|-)  cos  (S+p^) 
gr^  cos  (L+a^)  sin  (S+p^) 

■  -r 

yX  ,  3 

ECEF 

^r^  sin  (L+a^) 

(36) 


The  solution  is 


L+a^ 


tan 


-1 


(oxi*3)2 


(_x^''')2  + 


(L+a^)  s;  32  degrees 


(37) 


20 


(S+p^)  ~  106  degrees 


(38) 


„i.  1 

i  1  ° 

S+p-^  =  cos  - 

cos  (L+a^) 

where 

or^  =  — - —  (39) 

sin  (L+a^) 

The  results  are  given  in  Table  1  below. 


i 

Sensor 

Type 

di-T 

di.2 

1  3 

L+a^ 

S+pi 

ri 

1 

ps 

0.50 

0.50 

0. 

32 . 3872 

106.4724 

0. 

2 

ps 

0.25 

3.00 

0. 

32.4233 

106.4767 

0. 

3 

as 

2.00 

4.00 

0. 

32.4378 

106.4467 

0. 

4 

ps 

2.50 

2.00 

0. 

32.4089 

106.4582 

0. 

5 

as 

3.00 

1.00 

0. 

32.3944 

106.4296 

0. 

_ 

Table  1:  Passive  Sonar  (ps)  and  Active  Sonar  (as)  Locations  and 
Orientations  in  GCS  for  the  Underwater  Target 

Detailed  equations  that  describe  the  translational  motion  of  the  target 
were  developed.  The  equations  include  a  radial  gravitational  force  as  well  os 
centrifugal  and  Coriolis  forces  which  come  about  by  rotation  of  the  GCS  about 
the  polar  axis.  The  equations  are 


((x‘')2  +  (x2)2  +  (x3+r„)2)3/2 


((x'')2  +  (x2)2  +  (x3+r_)2)3/2 


-p(x3+r-) 


((x‘')2  +  (x2)2  +  (x3+ra)2)3/2 


+  2Q(x^sin  L  -  x®cos  L)  +  ff^yl  (40) 


2Qx^sin  L 


+  92(,(2gj^p2  L  _  (x^+r_)cos  L  sin  L)  (41) 


+  2Cx^cos  L 


+  92((x®+rQ)cos2  L  -  x^cos  L  sin  L)  (42) 


where 


P  •  G  iHg 


(43) 
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and  all  other  variables  are  defined  in  the  List  of  Symbols.  A  sphericol 
harmonic  expansion  of  the  earth’s  gravitational  field  is  not  necessary  since 
the  variation  in  target  depth  was  small  relative  to  the  earth’s  radius. 
However,  a  major  effect  is  hydrodynamic  drag  and  buayancy  which  we  accounted 
for  by  reducing  G  to  1/1000  of  its  correct  volue.  Thus,  the  trajectories 
tended  to  be  unstable  (due  to  centrifugal  forces  dominating)  but  reasonable 
ones  could  be  obtained  by  careful  tuning  of  the  initial  condition.  More 
precise  modeling  will  be  undertaken  in  Phase  II. 

Any  one  of  three  sensor  types  may  reside  at  each  sensor  location.  The 
three  types  are 

■  active  sonar  wherein  range  (r^),  azimuth  (P^)  and  elevation  (0^)  data 
are  available, 

•  passive  sonar  wherein  azimuth  and  elevation  data  are  available,  and 

•  doppler  sonar  wherein  range,  range  rate  (r^),  azimuth  and  elevation 
data  are  available. 

The  observational  equations  in  terms  of  the  global  state  are 

r^  =  ((x^-d^*^)^  +  (x^-d^’^)^  (x^-d^  ’  (44) 


[0  0  1]  (Ti)^*" 


0^  =  sin 


-1 
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x^-d^ ’ ^ 
x^-d^ • 3 


((x^-di’'')2  +  (x2-di’2)2  +  (x5_cji.3)2)1/2 


(45) 


[0  1  0]  (T^)^'' 


X  ^ -d^ ’ ^ 
x2-di-2 

x^-d^’ ^ 


=  tan 


-1 


[1  0  0]  (T^)^'" 


x^-d^’ ^ 
x2-df '2 
x^-d^' 3 


(46) 


.1  . 


[  X 
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X®  ] 


x2-d^’ 2 
x3_cji.  3 


((x^-d^*^)2  +  (x2-d^*2)2  +  (x^-d^’ ^)2)  V2 


(47) 
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The  various  measurement  variables  are  defined  by  Figure  1  that  follows. 


X 


i,  3 


Figure  1;  The  Local  Coordinate  System  and  Measurement  Variables 


The  nonlinear  equations  of  motion  {40)-(42)  were  integrated  forwards  in 
time  starting  from  (26)  and  using  the  Fourth  Order  Runge  Kutto  method  with  a 
1  second  interval.  Figures  2  and  3  show  the  resultant  trajectory  of  the 
target  vehicle.  As  a  further  check  on  the  Fortran  code  for  this  part  of  the 
simulation,  the  total  translational  energy  (kinetic  plus  potential)  of  the 
target  was  computed  for  each  point  along  the  computed  trajectory.  The  total 
energy  remained  constant  to  within  as  it  should  since  there  are  no 

external  forces  (u(t)  =  0)  and  the  system  is  conservative.  Figures  4 
through  8  show  some  of  the  corresponding  measurements  for  the  various  data¬ 
types  with  v^  =  0  for  (1<i<M}. 

Each  row  of  A(t)  is  computed  by  partial  differentiating  the  same  row  of 
f(x(t))  w.r.t.  the  nominal  state.  Thus,  A(t)  has  the  following  structure 


A(t) 
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0 
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0  0  10  0 
0  0  0  1  0 
0  0  0  0  1 


(48) 
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where 
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(58) 


31 
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(59) 
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Each  row  of  C^(t)  is  computed  by  partial  differentiating  the  same  row  of 
hi(x(t)  w.r.t.  the  nominal  state.  Thus,  when  the  ith  sensor  is  a  doppler 
sonar,  C^(t)  has  the  following  structure 
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Ci(t)  = 
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where 

((x^-di- ■')2  +  (x2_di'2)2  +  (x3_jii.3)2)1/2 
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^  ri  (x^(x^-d^>^)  +  x®(x^-d^’^)  +  x®(x^-d^ ’ ^) )(x^-d^ • ^) 
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x5 
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^  x^  ((x^-d^-'')2  +  (x2-d^'2)2  +  ^j(3_jji.3)2j1/2 
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where  u  is  the  argument  of  sin“^  in  equation  (45). 
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where  u  is  now  the  argument  of  ton  ^  in  equation  (46). 
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^  pi  ^  pi  ^  pi 


(86) 


The  6  biases  , d^ • ^ , d^ • ^, d^ • ^  per  local  system  could  be 

included  as  states  in  the  filter  and  estimated  in  order  to  correct  for  any 
preflight  miscalibration .  Then,  =  0,  =  0  . . .  would  be  added  to  the 

equations  of  motion  and  additional  partial  derivative  expressions  would  be 
needed.  However,  only  the  6  positions  and  velocities  of  the  underwater 
target  were  included. 

In  order  to  create  simulated  data  and  test  the  state  estimation  part  of 
the  code,  a  subroutine  RANDOM  for  generating  sequences  of  Gaussian  random 
vectors  with  prescribed  covariance  was  used.  Two  algorithms  were  considered 
in  the  derivation  of  the  subroutine.  The  first  proceeds  by  rotating 
coordinates  to  a  system  in  which  the  covariance  matrix  is  diagonal.  In  this 
system  the  multivariate  normal  density  becomes  equal  to  the  product  of  its 
marginal  densities,  and  each  marginal  density  can  be  sampled  independently  of 
the  other  components.  After  obtaining  a  sample  vector  in  this  rotated  system, 
the  coordinates  are  rotated  back  to  the  original  system. 

The  second  algorithm  proceeds  by  decomposing  the  multivariate  normal 
density  into  the  product  of  the  marginal  density  of  the  first  variate  times 
the  joint  density  of  the  remaining  variates,  conditional  upon  the  value 
sampled  for  the  first.  This  joint  density  is  determined  once  the  first 
variate  has  been  sampled  from  its  marginal  density.  The  procedure  is  then 
applied  to  the  second  variate  and  iterated  until  values  have  been  assigned  to 
all  components  of  the  sample  vector.  This  "Conditional  Decomposition 
Algorithm"  will  execute  more  rapidly  thon  the  latter  "Matrix  Diagonalization 
Algorithm"  especially  for  time  varying  covorionce  motrices.  Thus  it  was 
chosen  as  the  basis  for  subroutine  RANDOM. 

Assuming  a  constant  covariance  matrix,  RANDOM  has  been  tested  by  counting 
the  number  of  random  values  within  several  bands  for  each  component. 

Comparison  with  theory  has  shown  ogreement  to  within  a  few  percent. 

Figures  9  and  10  show  the  evolution  of  the  perturbed  state  as  governed  by 
equation  (3).  The  initial  condition  is 

Sxg  -  [  79.2ft  O.ft  O.ft  O.ft/s  O.ft/S  O.ft/s  ] 
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which  represents  a  perturbation  in  the  x  location  only.  However,  the 
resulting  perturbed  trajectories  in  the  y  and  z  directions  are  nonzero  due  to 
cross  coupling  terms  within  A(t).  A  diagonal  with  small  variances  was 
used  to  generate  the  process  noise  sequence.  The  position  variances  were 
10  ft^  and  the  velocity  variances  were  10“^  (ft/s)^. 

Figures  11,  12  and  13  are  the  corresponding  DSRIF  results  with  all  of  the 
prior  and  process  noise  information  embedded  in  the  merge  processor.  Each  of 
the  12  local  filters  processed  1  measurement  variable.  A  diagonal  ,  with 
variances  of  10“®  deg^  and  1  ft^  for  angular  variables  and  range  variables 
respectively,  was  used  to  generate  the  measurement  noise  sequence.  The 
initial  state  estimate  for  all  of  the  local  filters  was 

SxJ  =  [179.2ft  100. ft  10. ft  5.97  ft/s  5.97  ft/s  -.41  ft/s  ] 

and  a  diagonal  Po(+)i  with  variances  of  1  ft^  ond  1  (ft/s)^  for  positron 
variables  and  velocity  variables  respectively,  was  used  to  initialize  the 
merge  processor.  Figures  11  and  12  show  that  the  rms  position  estimate  errors 
quickly  decay  to  steady  state  mean  values  after  only  a  few  time  samples 
however,  the  velocity  estimate  errors  reach  steady  state  after  approximately 
40  samples.  The  corresponding  estimate  error  covariances  in  Figure  13  follow 
the  some  course  as  expected. 

In  Figure  14  the  process  noise  levels  were  multiplied  by  5  and  100  for 
position  and  velocity  variances  respectively.  Comparison  with  Figure  13  shows 
that  the  corresponding  estimate  error  covariances  increase  as  well.  This  is 
as  expected  since  is  linearly  related  to  the  time  updated  estimate  error 
covariance  i.e.,  the  conventional  covariance  time  update  equation  is  given  by 

Pk  +  i(-)  =  +  +  °k  (87) 

Furthermore,  the  same  phenomenon  results  when  is  multiplied  by  a  factor  of 
10®  for  angular  variables  and  10  for  range  variables  in  Figure  15.  The 
conventional  ccvariance  measurement  update  equation  in  Josephson  Stabilized 
form 

Pk(  +  )  =  [I  -  P^(-)  [I  -  +  Ki^R^K^tr  (88) 

may  be  combined  with  the  Kalman  gain  equation 

'<k  '  Pk(-)8k^''  t8kPk(-)Hk^''  +  Rkr"*  (89) 

to  show  that  the  time  updated  estimate  error  covariance  is  linearly  related  to 
as  well.  The  result  is  that 

Pk"’'<  +  )  -  Pk'''(-)  +  Hk^''  Rk"''  Rk  (90) 
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2.2  Extended-Decentrafized  Square  Root 
Information  Filtering  of  MLRS  Data 


Under  SBIR  Phase  I  contract  DAAD07-87-C-01 03  with  the  U.S.  Army,  White 
Sands  Missile  Range  (WSMR),  MTI  previously  derived  an  extended  version  of  the 
algorithm  and  successfully  used  it  to  track  real  Multiple  Launch  Rocket  System 
(MLRS)  data  provided  by  WSMR.  In  this  section  we  describe  some  of  the  results 
obtained  along  with  points  of  departure  for  future  work  that  was  needed. 

Then,  in  the  next  section  2.2.1  we  finish  processing  the  MLRS  data. 

On  November  11,  1987  six  rockets  were  launched  sequentially  in  time  over 
a  period  of  2  hours  and  30  minutes  at  WSMR.  Only  1  rocket  was  airborne  at  any 
one  time  and  thus  data  association  for  multitarget  tracking  was  not  needed. 

The  digitized  measurements  for  all  six  shots  were  plotted  in  order  to  select 
the  best  set  as  characterized  by  the  least  amount  of  data  drop  outs  and 
outlyers. 

The  MLRS  data  set  contained  azimuth  and  elevation  angle  measurements 
(with  respect  to  each  local  sensor)  from  11  optical  trackers  (analogous  to 
passive  hydrophones).  The  origin  of  the  White  Sands  Coordinate  System  is  at 
32.38  degrees  latitude  and  106.481  degrees  longitude.  The  data  set  also 
contained  range,  azimuth  and  elevation  angle  measurements  from  3  radars 
(analogous  to  active  sonar)  but  with  respect  to  the  local  coordinate  system 
originating  at  the  launcher.  The  precise  location  and  orientation  of  each 
sensor  was  known  a  priori  and  no  attempt  was  made  to  estimate  its  uncertainty. 

In  this  case,  an  extended  version  of  the  DSRIF  is  needed  since  the 
nominal  rocket  trajectories  were  unavailable.  The  same  is  true  for  underwater 
vehicle  tracking  although  the  velocities  and  accelerations  are  much  smaller. 
The  Extended  DSRIF  (E-DSRIF)  may  be  derived  by  extending  the  observations 
equation,  linearized  about  the  current  estimate,  to 


where 


and 


zi  = 


Yk  =  ^^k  ’‘k  +  ''k  +  ^k 

-  Xk^-) 

X«Xk(-) 


(91) 

(92) 


Hi 


^  hi{x) 


(93) 


X-X|.(-) 


and  extending  the  dynamics  equation,  linearized  about  the  current  estimate,  to 

’'k+l  "  '"k  »'k  +  +  g,^ 


A6 


(94) 


where 


=  f(.^)  -  Fk  +  ) 

x=x^(+) 

and 


f(x)  I 


ix=x^(+) 

Substituting  equations  (91)  and  (92)  into  the  least  squares  performance 
functional  of  equation  (13)  in  our  DARPA  Phase  I  proposal  gives  the  result 
(details  were  provided  in  [2]). 

Processing  on  the  global  scale  is  the  same  as  for  the  DSRIF  i.e.,  the 
merge  steps  are  exactly  as  defined  in  the  Phase  I  proposal.  Only  processing 
on  the  local  scale  is  different.  A  major  difference  between  the  E-DSRIF  and 
DSRIF  is  that  the  local  E-DSRIFilters  require  knowledge  of  the  globally 
optimal  estimate  Xj^(+)  in  order  to  compute  their  first  order  Taylor  series 
expansion  terms  F^,  g^^,  hJ  and  zj  whereas  the  DSRIF  may  compute  X|^(  +  )  at  any 
rate  less  than  the  highest  data  rate.  Future  research  should  examine  whether 
an  E-DSRIF  algorithm,  in  which  the  Taylor  series  expansions  are  about  the 
locally  optimal  estimates,  may  be  derived. 

In  order  to  derive  a  suitable  dynamical  model  as  well  os  initialize  the 
filter,  the  position,  velocity,  acceleration  and  jerk  of  the  rocket  were 
precomputed  using  finite  differencing  with  At  =  . 1  seconds.  Results  were 
plotted  in  Figures  29  through  30  (of  [2])  using  all  of  the  data  provided  for 
radar  #350  except  for  the  first  21  samples  (wo  estimated  that  all  radar 
trackers  or  rt's  had  locked  onto  the  target  by  the  22nd  sample).  Figure  30 
indicated  that  jerk  could  be  suitably  modeled  as  a  white  Gaussian  noise 
process  with  constant  moan.  Thus,  the  E-DSRIF  was  encoded  in  Fortran  ’77 
using  a  second  order  polynomial  dynamical  model. 

Figures  31  through  35  showed  the  rt  and  optical  tracker  (ot)  measurements 
as  predicted  by  the  E-DSRIF.  Comparison  with  the  actual  measurements  in 
Figures  16,  18,  19,  20  and  22  showed  an  exact  match  to  within  a  plotting  line 
width.  A  better  means  of  comparison  is  thus  provided  below  in  Tables  2  and  3 
where  r^  is  range  in  feet  is  azimuth  in  degrees,  and  6^  is  elevation 

in  degrees.  Also,  (m.e)  corresponds  to  (actual  measurements  ,  estimated 
measurements} . 

The  large  values  of  Rk(j,j)  for  ot  voriables  servos  to  weight  the  rt  data 
much  more  heavily  in  computing  estimates.  Decreasing  the  ot  measurement 
errors  to  more  realistic  values  should  give  similar  results  since  the  ' 
predicted  ot  measurements  matches  their  actual  values  very  closely. 
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012 
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4,355.61 

359.19 

18.74 

24 
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358.30 
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358. 16 

18.07 

4,658.07 

358.16 

18.07 

229 
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358.96 

11.88 

52,163.32 

358.95 
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52,167.32 

358.96 

11.89 

52, 167.32 

358.96 

11.89 

230 
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52,332.09 

358.96 

11.84 

52,324.25 

358.95 

11.86 

■ 
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52,328. 15 

358.96 

11.85 

52,328. 15 

358.96 

11.85 

459 

m 

80,328.90 

359.22 

2.96 

80, 178.71 

359.22 

3.00 

e 

80,254.10 

359.22 

2.98 

80,254.10 

359.22 

2.98 

Table  2:  Radar  #350  and  #394  Measurements  and  Estimated  Measurements  for 

MLRS.  j  I  j  ot  variables  and  for  rt  range, 

azimuth  and  elevation  variables  respectively. 
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229 
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22.93 

20.28 

331.64 

12.52 
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22.80 

20. 18 

331.73 

12.48 
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26.07 

5.58 

«  «« 

««« 

««« 

««« 
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26.32 

3.94 

10.98 

3.66 

342.28 

2.98 

Table  5;  G30,  G80,  G110  Measurements  and  Estimated  Measurements  for  MLRS. 

variables  and  for  rt  range,  azimuth 


and  elevation  variables  respectively. 


9.3  X  10‘'7  ] 


Figures  36  and  37  showed  the  global  position  estimates  and  corresponding 
estimate  error  covariances  respectively.  Again,  the  rocket  positions  derived 
from  radar  #350  as  compared  with  the  E-OSRIF  estimates  based  upon  all  of  the  5 
selected  sensors,  showed  extremely  close  ogreement.  The  slight  difference  in 
the  estimate  of  height  is  due  to  using  jlauch  ^  j  instead  of  its  correct 
value  as  defined  by  equation  (34)  from  [2].  In  Table  4  below,  the  estimates 
are  compared  using  the  correct  coordinate  transformation. 
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184. 

m 

-15,202. 
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866. 

-439. 

.9 

-25. 
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Table  4: 


Global  Position  Estimates  and  Derived  Measurements  for  MLRS. 

j  <  j  ot  variables  and  for  rt  range,  azimuth 

and  elevation  variables  res 
Qk^J'J  J  [  1 .7  X  10 


spectively . 

7.4  X  lO'''^ 


9.3  X  10 


17 


Finally,  the  monotonically  increasing  estimate  error  covariance 

(actually,  Pq  -  diog  [  10^^  ft^  _  10^^  ft^/sec^  ...  10^^  ft^/sec^  ...  ] 

was  used  to  initialize  the  covariance  propagation  so  that  the  first  step  is  a 
large,  off  scale  decrease  to  approximately  100  ft^)  is  due  to  our  using  values 
of  approximately  12  orders  of  magnitude  higher  than  its  precomputed  sample 
value.  A  more  realistic  value  should  result  in  a  P|^  with  quite  the  opposite 
behavior. 
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2.2.1  New  Results 


The  E-DSRIF  was  successfully  used  to  track  an  MLRS  data  set  in  previous 
work  however,  a  monotonically  increasing  estimate  error  covariance  was 
observed  (  see  Figure  37  of  [2]).  This  is  due  to  our  using  values  of 
many  orders  of  magnitude  higher  than  its  precomputed  sample  value  in  order  to 
compensate  for  any  errors  in  the  model.  For  this  subtask,  was  adjusted 

interactively  until  an  acceptable  covariance  function  was  obtained.  As 
Figures  A  through  C  indicate,  more  realistic  values  of  Q|^  results  in  a 
with  quite  the  opposite  (and  more  desirable)  behavior  previously  illustrated. 


starts  high  at  10^  in  Figures  A  through  C  and  decreases  monotonically 
to  Both  the  position  and  velocity  estimate  error  variances  are  still 

unstable  when  =  10^  however  both  reach  stability  when  Q|^  =  10”^.  Not 
much  improvement  is  gained  by  decreasing  beyond  10“^.  One  of  the 

measurement  variables  from  each  of  the  5  sensors  selected  were  also  plotted 
(not  shown  here)  in  Figures  A  through  C.  This  was  done  in  order  to  see 
whether  the  oscillations  in  estimate  error  varionces,  which  begin  to  occur 
when  =  10“^  ,  are  correlated  with  the  time  intervals  of  data  drop-out. 

Next,  the  ot  measurement  errors  were  kept  artificially  high  in  Phase  I. 
These  large  values  of  R(^  for  ot  variables  serves  to  weight  the  rt  data  much 
more  heavily  in  computing  estimates.  In  Figures  D  through  F  (where  Q^=10^) 
and  G  through  I  (whore  «  10“^),  R|^  was  decreased  for  5  of  the  6  ot 
measurement  variables.  Decreasing  some  of  the  ot  measurement  errors  from  10^® 
to  more  realistic  values  near  1  improved  the  results  significantly  by 
increasing  the  rate  of  convergence,  especially  near  the  initial  time  point. 

Finally,  an  adaptive  form  of  the  E-DSRIF  wherein 


0^  =  factor  •  P^(+) 


(97) 


was  encoded.  This  simple  method  for  tuning  the  filter  by  using  a  feedback 
loop  to  compute  gave  exceedingly  good  results  as  shown  in  Figures  J 
through  L  (where  all  measurement  errors  were  chosen  to  be  the  nominal  Phase  I 
values).  Best  results  were  obtained  using  a  scale  factor  of  .001,  and  this 
was  used  in  the  last  set  of  Figures  M  through  0.  In  this  latter  set, 
again  was  manually  decreased  for  3  of  the  6  ot  measurement  variables. 

Overall,  the  best  results  (smallest  estimate  errors)  ore  shown  in  Figures  M 
and  0,  for  the  cose  R(^(j,j)  -  10“^  for  ot  variables.  Adaptation  by  direct 
feedback  of  P|((+)  is  a  loss  sophisticated  means  of  estimating  Qk  in  real 
time  than  the  method  of  maximum  likelihood  albeit,  it  is  much  simpler  to 
implement . 


3.0  Estimates  of  Technical  Feasibgity 

VLSI  (Very  Large  Scale  Integration)  technology  has  been  developed  to  the 
point  where  high  speed  floating  point  processors  may  be  concatenated  to  form 
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compact  supercomputers  with  for  greater  throughput  than  uniprocessor  machines. 
Thus,  there  is  considerable  interest  within  the  signal  processing  community  in 
the  development  of  parallel  versions  of  conventional  algorithms.  MTI  has 
collaborated  with  Dr.  G.J.  Bierman  to  develop  a  parallel  form  of  the  Kalman 
filter  that  has  several  very  unique  and  important  features.  We  believe  that 
utilization  of  these  features  will  result  in  the  design  of  an  integrated 
tracking  system  that  exhibits  much  improved  performance  over  any  "isolated" 
approach  to  undersea  surveillance  or  test  range  tracking. 

Specifically,  our  Decentralized  Square  Root  Information  Filter  (DSRIF) 

[1]  allows  each  group  of  measurement  variables,  the  process  noise  statistics 
and  the  prior  information  about  the  initial  state  to  be  processed  in  separate 
but  locally  optimal  filters.  Globally  optimal  state  estimates  and  estimate 
error  covariances  may  then  be  computed  by  combining  local  filter  outputs  on 
demand.  This  will  allow  the  analyst  to  identify  the  contribution  of  each 
measurement  group,  the  process  noise  and  the  prior  information  about  the 
initial  state  to  the  global  state  estimate  and  estimate  error  covariance 
without  additional  computation. 

Furthermore,  the  process  noise  and  prior  information  may  be  distributed 
amongst  the  data  processing  filters  in  order  to  improve  upon  the  fault 
tolerant  characteristics  of  the  nominal  algorithm  when  real-time  signal 
processing  is  an  issue.  In  this  case,  the  estimates  and  covariances  should 
gracefully  degrade  fram  global  optimality  as  local  processors  fail.  Thirdly, 
the  algorithm  is  based  upon  numerically  reliable  matrix  factorization  methods 
which,  unlike  the  CKF,  will  never  fail. 

The  objective  of  Phase  I  research  was  to  validate  the  DSRIF  equations  by 
testing  its  ability  to  track  both  predetermined  and  unknown  trajectories  when 
perturbed  by  white  Gaussian  noise.  The  state  estimates  and  error  covariances 
obtained  were  found  to  be  identical  (when  printed  to  10  significant  digits) 
with  those  of  a  SRIF  implemented  in  centrolized  form  with  all  calculations 
performed  in  double  precision  arithmetic.  Furthermore,  an  adaptive  version  of 
the  extended  DSRIF  (E-DSRIF)  was  successfully  used  to  track  real  Multiple 
Rocket  Launch  System  data  obtained  from  the  WSMR. 

In  order  to  determine  the  feasibility  of  our  distributed  approach  to 
multisensor  tracking,  several  specific  technical  objectives  must  be  met. 

First  and  faremost,  the  basic  DSRIF  theory  needs  to  be  extended  to  enable  the 
tracking  of  multiple  targets.  This  requires  that  a  theory  for  associating 
data  with  targets,  based  upon  the  DSRIF  (and  not  the  SRIF  which  already 
exists,  see  [2]),  be  developed.  Correlation  of  measurements  with  targets  can 
best  be  done  using  a  hypothesis  testing  approach.  The  idea  is  to  select  the 
correlation  of  measurements  with  targets  that  has  maximum  probability  given 
the  data.  Calculation  of  all  combinations  to  form  the  entire  set  of  these 
conditional  probabilities  can  be  prohibiting,  especially  in  a  dense  target 
environment.  A  mo.lor  advantoqe  in  using  the  DSRIF  is  the  tremendous  reduction 
in  computational  cost  ossociated  with  this  calculotion. 

Secondly,  the  DSRIF  is  a  new  algorithm  which  has  undergone  only  limited 
testing  in  Phase  I  research.  Extensive  testing  within  a  multisensor 
multitarget  tracking  scenario  is  needed.  Other  theoretical  questians  such  as 
the  development  of  a  delayed-state  DSRIF  for  processing  range-rate 
measurements,  a  method  for  isolating  faulty  sensors,  and  efficient 
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implementations  of  the  DSRIF  that  facilitate  high  data  rates  need  to  be 
addressed . 


Finally,  consideration  needs  to  be  given  to  the  design  of  the  tracking 
network  both  at  the  global  and  local  levels.  The  major  question  here  is 
whether  a  sufficient  data  rate  can  be  achieved  using  current  chip  technology. 
Another  question  is  whether  the  architecture  can  be  reconfigured  (in  software) 
to  implement  other  members  of  the  family  of  DSRIFs.  A  multitude  of  test  range 
scenarios  is  envisioned  so  that  a  robust  system  is  needed.  At  one  extreme, 
test  vehicles  may  include  slow  moving  submarines  with  well  defined  nominal 
trajectories  a  priori  while  at  the  other,  multiple  smart  torpedoes  with 
maneuvering  capability  is  possible.  The  key  to  a  successful  network  design  is 
to  employ  a  more  or  less  sophisticated  version  of  the  algorithm  depending  upon 
the  particular  scenario.  Thus  the  network  must  be  adaptable.  For  example, 
preflight  simulations  of  the  proposed  shot  using  high  fidelity  hydrodynamic 
models  can  yield  good  values  for  the  process  noise  levels  and  a  basic  DSRIF 
should  result  in  good  tracking  performance.  However,  a  sudden  departure  from 
the  nominal  trajectory  would  require  a  detection  mechanism  as  part  of  the 
algorithm  and  adjustment  of  in  real  time.  A  DSRIF  based  multisensor 
laboratory  tracking  experiment  should  be  performed  prior  to  deployment  of  a 
test  range  prototype. 
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